#line 1 "D:/Archivio/mikroC/tric-ricotta/sensors.c"
#line 1 "d:/archivio/mikroc/tric-ricotta/config.h"
#line 1 "d:/archivio/mikroc/tric-ricotta/lib_bma180.h"
#line 11 "d:/archivio/mikroc/tric-ricotta/lib_bma180.h"
void BMA180_init();
void BMA180_readAcc(float *acc);
#line 1 "d:/archivio/mikroc/tric-ricotta/lib_itg3200.h"
#line 11 "d:/archivio/mikroc/tric-ricotta/lib_itg3200.h"
void ITG3200_init();
void ITG3200_readGyro(float *gyro);
#line 1 "d:/archivio/mikroc/tric-ricotta/sensors.h"



void SENSORS_init();
void SENSORS_read(float *SENSORS_acc, float *SENSORS_gyro);
void SENSORS_process(float *SENSORS_acc, float *SENSORS_gyro, float *SENSORS_angle);
float SENSORS_complementaryFilter(float oldAngle, float newAngle, float newRate);
#line 13 "D:/Archivio/mikroC/tric-ricotta/sensors.c"
void SENSORS_init() {
 I2C1_Init(400000);
 Delay_ms(500);

 ITG3200_init();
 Delay_ms(500);
 BMA180_init();
 Delay_ms(500);
}

void SENSORS_read(float *SENSORS_acc, float *SENSORS_gyro) {
 BMA180_readAcc(&SENSORS_acc);
 ITG3200_readGyro(&SENSORS_gyro);
}

void SENSORS_process(float *SENSORS_acc, float *SENSORS_gyro, float *SENSORS_angle) {
 float vector;

 vector = sqrt(SENSORS_acc[0]*SENSORS_acc[0] + SENSORS_acc[1]*SENSORS_acc[1] + SENSORS_acc[2]*SENSORS_acc[2]);
 SENSORS_angle[0] = SENSORS_complementaryFilter(SENSORS_angle[0], asin(SENSORS_acc[1]/vector)*57.295779, -SENSORS_gyro[0]);
 SENSORS_angle[1] = SENSORS_complementaryFilter(SENSORS_angle[1], asin(SENSORS_acc[0]/vector)*57.295779, SENSORS_gyro[1]);
 SENSORS_angle[2] += (SENSORS_gyro[2]* 0.015 );
}

float SENSORS_complementaryFilter(float oldAngle, float newAngle, float newRate) {
 float filterK;
 float filteredAngle;

 filterK =  0.05  / ( 0.05  +  0.015 );
 filteredAngle = filterK* (oldAngle + newRate *  0.015 ) + (1-filterK) * (newAngle);
 return filteredAngle;
}
